#ifndef ATTITUDE_CONTROL_NODE_PX4_HPP
#define ATTITUDE_CONTROL_NODE_PX4_HPP

#include <rclcpp/rclcpp.hpp>
#include <mavros_msgs/msg/actuator_control.hpp>
#include <mavros_msgs/srv/set_mode.hpp>
#include <mavros_msgs/srv/command_bool.hpp>
#include <mavros_msgs/msg/state.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <std_msgs/msg/float64.hpp>
#include <fstream>
#include <string>
#include "pid_controller.hpp"

struct ControlParams {
    double kp_boat_angle = 1.0;
    double attitude_angle = 0.0;
    bool use_3d_angle = true;
    int flag_control = 0;  // 0: attitude control, 1: vision control
    std::string csv_path = "boat_data";
    double default_throttle_output = 0.5;
};

struct TimeoutConfig {
    double dock_timeout = 5.0;
    double boat_timeout = 2.0;
};

class AttitudeControlNodePX4 : public rclcpp::Node {
public:
    AttitudeControlNodePX4();
    ~AttitudeControlNodePX4();
    
    void run();

private:
    // ROS callbacks
    void stateCallback(const mavros_msgs::msg::State::SharedPtr msg);
    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
    void globalCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
    void velocityCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
    void dockAngle3DCallback(const std_msgs::msg::Float64::SharedPtr msg);
    void dockAngle2DCallback(const std_msgs::msg::Float64::SharedPtr msg);
    void boatAngleCallback(const std_msgs::msg::Float64::SharedPtr msg);
    
    // Timer callbacks
    void checkDockTimeout();
    void checkBoatTimeout();
    
    // Parameter callback
    rcl_interfaces::msg::SetParametersResult parametersCallback(
        const std::vector<rclcpp::Parameter> &parameters);
    
    // Control functions
    void updateDesiredYaw();
    void sendActuatorControl();
    void detectAndCompensateYawJump();
    void setOffboardMode();
    void armDrone();
    
    // Utility functions
    void quaternionToEuler(const geometry_msgs::msg::Quaternion& q, double& roll, double& pitch, double& yaw);
    void loadParameters();
    void setupPublishersSubscribers();
    void initializeDataLogging();
    void logData();
    std::string getCurrentTimeString();
    
    // ROS communication
    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr global_sub_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr dock_angle_3d_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr dock_angle_2d_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr boat_angle_sub_;
    
    rclcpp::Publisher<mavros_msgs::msg::ActuatorControl>::SharedPtr actuator_pub_;
    
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
    
    rclcpp::TimerBase::SharedPtr dock_timer_;
    rclcpp::TimerBase::SharedPtr boat_timer_;
    rclcpp::TimerBase::SharedPtr control_timer_;
    
    // Parameter callback handle
    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
    
    // Control parameters and state
    ControlParams control_params_;
    TimeoutConfig timeout_config_;
    PIDController yaw_pid_;
    PIDController throttle_pid_;
    mavros_msgs::msg::State current_state_;
    
    // Vehicle state
    double current_yaw_ = 0.0;
    double desired_yaw_ = 0.0;
    double current_yaw_record_ = 0.0;
    bool current_yaw_record_flag_ = true;
    double previous_yaw_ = 0.0;
    bool first_yaw_update_ = true;
    
    // Vision angles
    double dock_angle_ = -1.0;
    double dock_angle_2d_ = 0.0;
    double dock_angle_3d_ = 0.0;
    double boat_angle_ = 0.0;
    
    // Position and orientation
    double position_x_ = 0.0, position_y_ = 0.0, position_z_ = 0.0;
    double roll_angle_ = 0.0, pitch_angle_ = 0.0, yaw_angle_ = 0.0;
    double latitude_ = 0.0, longitude_ = 0.0;
    double v_body_x_ = 0.0;
    
    // Control outputs
    double yaw_output_ = 0.0;
    double throttle_output_ = 0.5;
    
    // Timing and flags
    rclcpp::Time last_dock_time_;
    rclcpp::Time last_boat_time_;
    int update_counter_ = 0;
    
    // Data logging
    std::ofstream log_file_;
};

#endif // ATTITUDE_CONTROL_NODE_PX4_HPP